#pragma once
#include "Array.hpp"
#include <cmath>

template< class _Ty >
class MyMatrix
{
private:
  Array< Array< _Ty >* > _Data;
  uint _Row;
  uint _Col;

public:
  MyMatrix () : _Row (), _Col () {}
  MyMatrix (unsigned int row, unsigned int column);
  MyMatrix (const MyMatrix< _Ty >& right);
  ~MyMatrix ();

  MyMatrix< _Ty >& operator= (const MyMatrix< _Ty >& right);
  MyMatrix< _Ty > operator+ (const MyMatrix< _Ty >& right);
  MyMatrix< _Ty > operator- (const MyMatrix< _Ty >& right);
  MyMatrix< _Ty > operator* (const MyMatrix< _Ty>& right);
  MyMatrix< _Ty > operator* (_Ty number);
  Array< _Ty >& operator[] (unsigned int row) const;

  // Get the amount of columns
  unsigned int column_num () const { return _Col; }

  // Get the amount of rows
  unsigned int row_num () const { return _Row; }

  bool push_row (const Array< _Ty >& row);
  bool pop_row ();

  bool push_col (const Array< _Ty >& col);
  bool pop_col ();

  bool insert_row (const Array< _Ty >& row, uint index);
  bool erase_row (uint index);

  bool insert_col (const Array< _Ty >& col, uint index);
  bool erase_col (uint index);

  void resize (uint row, uint col);
  void clear () { resize (0, 0); }

  bool exchange_rows (uint fstRow, uint secRow);
  bool add_rows (uint fromRow, uint toRow, _Ty k);
  bool row_multiply_by (uint row, _Ty k);

  bool calc_equation (OUT Array< _Ty >& result);
};


/************************************************************************/
/* Constructors and Destructor
/************************************************************************/

template< class _Ty >
MyMatrix< _Ty >::MyMatrix (unsigned int row, unsigned int column)
: _Row (0), _Col (0)
{
  resize (row, column);
}

template< class _Ty >
MyMatrix<_Ty>::MyMatrix (const MyMatrix< _Ty >& right)
: _Row (0), _Col (0)
{
  (*this) = right;
}

template< class _Ty >
MyMatrix<_Ty>::~MyMatrix ()
{
  resize (0, 0);
}


/************************************************************************/
/* Operators
/************************************************************************/

template< class _Ty >
MyMatrix< _Ty >& MyMatrix<_Ty>::operator= (const MyMatrix< _Ty >& right)
{
  resize (0, 0);
  _Col = right._Col;
  for (uint i = 0; i < right._Row; ++i)
    push_row (right[i]);
  return *this;
}

template< class _Ty >
MyMatrix< _Ty > MyMatrix<_Ty>::operator+ (const MyMatrix< _Ty >& right)
{
  if (_Row != right._Row || _Col != right._Col)
    throw ("Can't add two matrix with different size.");
  MyMatrix< _Ty > ans (_Row, _Col);
  for (uint i = 0; i < _Row; ++i)
    for (uint j = 0; j < _Col; ++j)
      ans._Data[i][j] = (*_Data[i])[j] + right[i][j];
  return ans;
}

template< class _Ty >
MyMatrix< _Ty > MyMatrix<_Ty>::operator- (const MyMatrix< _Ty >& right)
{
  if (_Row != right._Row || _Col != right._Col)
    throw ("Can't minus two matrix with different size.");
  MyMatrix< _Ty > ans (_Row, _Col);
  for (uint i = 0; i < _Row; ++i)
    for (uint j = 0; j < _Col; ++j)
      ans._Data[i][j] = (*_Data[i])[j] - right[i][j];
  return ans;
}

template< class _Ty >
MyMatrix< _Ty > MyMatrix<_Ty>::operator* (_Ty number)
{
  MyMatrix< _Ty > ans (_Row, _Col);
  for (uint i = 0; i < _Row; ++i)
    for (uint j = 0; j < _Col; ++j)
      ans[i][j] = (*_Data[i])[j] * number;
  return ans;
}

template< class _Ty >
MyMatrix< _Ty > MyMatrix<_Ty>::operator* (const MyMatrix< _Ty> &right)
{
  if (_Col != right._Row)
    throw ("Can't multiply!");

  MyMatrix< _Ty > ans (_Row, right._Col);
  for (uint row = 0; row < _Row; ++row)
    for (uint column = 0; column < right._Col; ++column)
      for (uint i = 0; i < _Col; ++i)
         ans[row][column] += (*_Data[row])[i] * right[i][column];
}

template< class _Ty >
Array< _Ty >& MyMatrix< _Ty >::operator[] (unsigned int row) const
{
  if (row > _Row) throw ("Out of Range.");
  return (*_Data[row]);
}


/************************************************************************/
/* Matrix Size Modifiers
/************************************************************************/

template< class _Ty >
bool MyMatrix< _Ty >::push_row (const Array< _Ty >& row)
{
  if (row.size () != _Col) return false;
  Array< _Ty >* new_row = new Array< _Ty > (row);
  _Data.push_back (new_row);
  ++_Row;
  return true;
}

template< class _Ty >
bool MyMatrix< _Ty >::pop_row ()
{
  if (_Row == 0) return false;
  --_Row;
  delete _Data[_Row];
  _Data.pop_back ();
  return true;
}

template< class _Ty >
bool MyMatrix< _Ty >::push_col (const Array< _Ty >& col)
{
  if (col.size () != _Row) return false;
  ++_Col;
  for (uint i = 0; i < _Row; ++i)
    _Data[i]->push_back (col[i]);
}

template< class _Ty >
bool MyMatrix< _Ty >::pop_col ()
{
  if (_Col == 0) return false;
  --_Col;
  for (uint i = 0; i < _Row; ++i)
    _Data[i]->pop_back ();
}

template< class _Ty >
bool MyMatrix< _Ty >::insert_row (const Array< _Ty >& row, uint index)
{
  if (row.size () != _Col || index >= _Row) return false;
  Array< _Ty >* new_row = new Array< _Ty > (row);
  _Data.insert (new_row, index);
  ++_Row;
  return true;
}

template< class _Ty >
bool MyMatrix< _Ty >::erase_row (uint index)
{
  if (_Row == 0 || index >= _Row) return false;
  --_Row;
  delete _Data[index];
  _Data.erase (index);
  return true;
}

template< class _Ty >
bool MyMatrix< _Ty >::insert_col (const Array< _Ty >& col, uint index)
{
  if (col.size () != _Row || index >= _Col) return false;
  ++_Col;
  for (uint i = 0; i < _Row; ++i)
    _Data[i]->insert (col[i], index);
}

template< class _Ty >
bool MyMatrix< _Ty >::erase_col (uint index)
{
  if (_Col == 0 || index >= _Col) return false;
  --_Col;
  for (uint i = 0; i < _Row; ++i)
    _Data[i]->erase (index);
}

template< class _Ty >
void MyMatrix< _Ty >::resize (uint row, uint col)
{
  while (_Row > row)
    pop_row ();
  for (uint i = 0; i < _Row; ++i)
    _Data[i]->resize (col);
  _Col = col;
  Array< _Ty > a_row (col);
  a_row.resize (col);
  while (_Row < row)
    push_row (a_row);
}

/************************************************************************/
/* Row Calculation
/************************************************************************/

//************************************
// Method: MyMatrix<_Ty>::row_multiply_by
// Usage: row = k * row
// Access:    public 
// Returns:   (bool)Whether the operation is success or not
//************************************
template< class _Ty >
bool MyMatrix<_Ty>::row_multiply_by (uint row, _Ty k)
{
  if (row >= _Row)
    return false;
  for (uint c = 0; c < _Col; ++c)
    (*this)[row][c] *= k;
  return true;
}


//************************************
// Method: MyMatrix<_Ty>::add_rows
// Usage: toRow += k * fromRow
// Access:    public 
// Returns:   (bool)Whether the operation is success or not
//************************************
template< class _Ty >
bool MyMatrix<_Ty>::add_rows (uint fromRow, uint toRow, _Ty k)
{
  if (fromRow >= _Row || toRow >= _Row || fromRow == toRow)
    return false;
  for (uint c = 0; c < _Col; ++c)
  {
    (*this)[toRow][c] += k * (*this)[fromRow][c];
  }
  return true;
}

//************************************
// Method: MyMatrix<_Ty>::exchange_rows
// Usage: Exchange between fstRow and secRow
// Access:    public 
// Returns:   (bool)Whether the operation is success or not
//************************************
template< class _Ty >
bool MyMatrix<_Ty>::exchange_rows (uint fstRow, uint secRow)
{
  if (fstRow >= _Row || secRow >= _Row)
    return false;
  if (fstRow == secRow)
    return true;
  for (uint c = 0; c < _Col; ++c)
  {
    _Ty temp = (*this)[fstRow][c];
    (*this)[fstRow][c] = (*this)[secRow][c];
    (*this)[secRow][c] = temp;
  }
  return true;
}





//************************************
// Method: MyMatrix<_Ty>::calc_equation
// Usage: Calculate the result of equation whose ceof-matrix is this matrix
//        The result will be stored in 'result' if success
// Access:    public 
// Returns:   (bool) Whether this equation has a unique result
//************************************
template< class _Ty >
bool MyMatrix<_Ty>::calc_equation (OUT Array< _Ty >& result)
{
  if (_Row == 0 || _Col <= 1)
    return false;
  uint cHead = 0;
  for (uint r = 0; r < _Row; ++r, ++cHead)
  {
    uint rTemp;
    while (cHead < _Col - 1) // The last column is const item
    {
      rTemp = r;
      while (rTemp < _Row && (*this)[rTemp][cHead] == 0)
        ++rTemp;
      if (rTemp != _Row)
        break;
      ++cHead;
    } // End while
    if (cHead >= _Col - 1)
      break; // Break For Loop
    exchange_rows (r, rTemp);
    for (uint j = 0; j < _Row; ++j)
    {
      add_rows (r, j, -(*this)[j][cHead] / (*this)[r][cHead]);
    }
    row_multiply_by (r, 1.0f / (*this)[r][cHead]);
  } // End for

  result.resize (_Col - 1);
  for (int r = _Row - 1; r >= 0; --r)
  {
    bool hasNoneZeroItem = false;
    for (uint c = 0; c < _Col - 1; ++c)
    {
      if ((*this)[r][c] > 0.01f)
      {
        if (hasNoneZeroItem)
          return false; // Result is not unique
        if ((*this)[r][c] < 0.99f || (*this)[r][c] > 1.01f)
          throw ("Need to be debug");
        result[c] = (*this)[r][_Col - 1];
        hasNoneZeroItem = true;
      }
    }
    if (!hasNoneZeroItem && abs ((*this)[r][_Col - 1]) > 0.01)
      return false; // No result
  }
  return true;
}